#include "vsf.h"
#include "core.h"

static struct vsfhal_info_t vsfhal_info =
{
	0, CORE_VECTOR_TABLE, 
	CORE_CLKEN, CORE_CLKSRC, MM32F103_PLLSRC_HSE,
	HSE_FREQ_HZ, HSI_FREQ_HZ, CORE_PLL_FREQ_HZ,
	CORE_AHB_FREQ_HZ, CORE_APB1_FREQ_HZ, CORE_APB2_FREQ_HZ, CORE_SYS_FREQ_HZ,
};

vsf_err_t vsfhal_core_get_info(struct vsfhal_info_t **info)
{
	*info = &vsfhal_info;
	return VSFERR_NONE;
}

vsf_err_t vsfhal_core_reset(void *p)
{
	NVIC_SystemReset();
	return VSFERR_NONE;
}

static uint32_t __log2__(uint32_t n)
{
	uint32_t i, value = 1;
	
	for (i = 0; i < 31; i++)
	{
		if (value == n)
			return i;
		value <<= 1;
	}
	return 0;
}

vsf_err_t vsfhal_core_init(void *p)
{
	uint32_t div;
	if (p != NULL)
		vsfhal_info = *(struct vsfhal_info_t *)p;

	RCC->CR |= RCC_CR_HSION;
	// TODO: clear ADCPRE
	RCC->CFGR &= ~(RCC_CFGR_SW | RCC_CFGR_HPRE | RCC_CFGR_PPRE1 | RCC_CFGR_PPRE2 | RCC_CFGR_MCO);
	RCC->CR &= ~(RCC_CR_HSEON | RCC_CR_CSSON | RCC_CR_PLLON);
	RCC->CR &= ~RCC_CR_HSEBYP;
	// TODO: clear PLLMUL
	RCC->CFGR &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_USBPRE);
	RCC->CR &= ~(RCC_CR_PLLON | RCC_CR_PLLRDY | RCC_CR_PLLDN | RCC_CR_PLLDM);
	RCC->CIR = RCC_CIR_LSIRDYC | RCC_CIR_LSERDYC | RCC_CIR_HSIRDYC | RCC_CIR_HSERDYC | RCC_CIR_PLLRDYC;

	if (vsfhal_info.clk_enable & MM32F103_CLK_HSE)
	{
		RCC->CR |= RCC_CR_HSEON;
		while (!(RCC->CR & RCC_CR_HSERDY));
	}

	if (vsfhal_info.clk_enable & MM32F103_CLK_PLL)
	{
		uint32_t clksrc, clkdst;
		uint8_t m, n;

		RCC->CFGR |= (vsfhal_info.pllsrc & 1) << 16;
		if (vsfhal_info.pllsrc == MM32F103_PLLSRC_HSID4)
			clksrc = vsfhal_info.hsi_freq_hz / 4;
		else if (vsfhal_info.pllsrc == MM32F103_PLLSRC_HSE)
			clksrc = vsfhal_info.hse_freq_hz;
		else
			return VSFERR_FAIL;

		for (m = 1; m <= 8; m++)
		{
			n = m * vsfhal_info.pll_freq_hz / clksrc;
			if (n > 0)
			{
				clkdst = clksrc * n / m;
				if (clkdst == vsfhal_info.pll_freq_hz)
					break;
			}
		}

		if (m > 8) return VSFERR_FAIL;
		RCC->CR |= ((m - 1) << 20) | ((n - 1) << 26);
		RCC->CR |= RCC_CR_PLLON;
		while (!(RCC->CR & RCC_CR_PLLRDY));
	}

	// set flash latency, current clock is HSI, so any latency is valid
	FLASH->ACR |= FLASH_ACR_PRFTBE;
	FLASH->ACR &= ~FLASH_ACR_LATENCY;
	if (vsfhal_info.cpu_freq_hz <= 24000000) {}
	else if (vsfhal_info.cpu_freq_hz <= 48000000)
		FLASH->ACR |= 1;
	else if (vsfhal_info.cpu_freq_hz <= 72000000)
		FLASH->ACR |= 2;
	else
		FLASH->ACR |= 3;

	// current clock is HSI, so any division is valid
	div = __log2__(vsfhal_info.cpu_freq_hz / vsfhal_info.ahb_freq_hz);
	if (div) RCC->CFGR |= (0x08 | ((div - 1) & 0x7)) << 4;
	div = __log2__(vsfhal_info.ahb_freq_hz / vsfhal_info.apb1_freq_hz);
	if (div) RCC->CFGR |= (0x04 | ((div - 1) & 0x3)) << 8;
	div = __log2__(vsfhal_info.ahb_freq_hz / vsfhal_info.apb2_freq_hz);
	if (div) RCC->CFGR |= (0x04 | ((div - 1) & 0x3)) << 11;

	// set the real clock
	RCC->CFGR |= vsfhal_info.clksrc;
	while ((RCC->CFGR & RCC_CFGR_SWS) != (vsfhal_info.clksrc << 2));

	SCB->VTOR = vsfhal_info.vector_table;
	NVIC_SetPriorityGrouping(vsfhal_info.priority_group);
	return VSFERR_NONE;
}

#ifdef F103_O_VERSION
vsf_err_t vsfhal_afio_config(const struct vsfhal_afio_pin_t *pin, uint32_t mode)
{
	GPIO_TypeDef *GPIOx = (GPIO_TypeDef *)(GPIOA_BASE + (pin->port << 10));
	RCC->APB2ENR |= RCC_APB2ENR_IOPAEN << pin->port;

	if (pin->af >= 0)
	{
		GPIOx->AFS[pin->pin >> 3] &= ~((uint32_t)0xF << ((pin->pin & 7) << 2));
		GPIOx->AFS[pin->pin >> 3] |= (uint32_t)pin->af << ((pin->pin & 7) << 2);
	}
	vsfhal_gpio_config(pin->port, pin->pin, mode);
	return VSFERR_NONE;
}
#endif

#include "hal/arch/arm_cm/arm_cm.h"
